#include "RectangleFinder.h"
#include "MiscFunctions.h"

#include <cmath>
#include <stdio.h>

using namespace std;
using namespace idch;

LineSegmentF swapEndPoints( const LineSegmentF& o )
{
    LineSegmentF ret;
    ret.x0 = o.x1;
    ret.y0 = o.y1;
    ret.x1 = o.x0;
    ret.y1 = o.y0;

    return ret;
}

RectangleFinder::RectangleFinder( double angleTol , double closeTol )
{
    this->angleTol = angleTol;
    this->closeTol = closeTol;

}

RectangleFinder::~RectangleFinder()
{

}

void RectangleFinder::process( const std::vector<LineSegmentF>& lines )
{
    foundRect.clear();

//    printf("----------------- BEGIN RECT FINDER ----------------------------------\n");
    segmentByAxis(lines);

//    printf(" vert = %d horiz = %d\n",(int)vert.size(),(int)horiz.size());

    // make sure there are enough lines for there to be a rectangle
    if( vert.size() < 2 || horiz.size() < 2 )
        return;

    for( size_t i = 0; i < vert.size(); i++ ) {

        std::vector<LineSegmentF> parallel;
        std::vector<LineSegmentF> perpendicular;

        LineSegmentF &v = vert.at(i);

//        printf("****** vert %d = %f %f %f %f\n",(int)i,v.x0,v.y0,v.x1,v.y1);

        // Find all lines that are parallel and end at about the same location on the y-axis
        for( size_t j = i+1; j < vert.size(); j++ ) {
            LineSegmentF &v2 = vert.at(j);

            //printf("acute angle %d -> %d = %f\n",(int)i,(int)j,acuteAngle(v,v2));

            // are they parallel?
            if( acuteAngle(v,v2) < angleTol ) {
                // see if their end points are close to each other along y-axis
                if( abs<double>(v.y0-v2.y0) < closeTol &&
                    abs<double>(v.y1-v2.y1) < closeTol ) {
                    parallel.push_back(v2);
                }
            }
        }

//        printf(" %d parallel = %d\n",(int)i,(int)parallel.size());
        // make sure this can result in a solution
        if( parallel.size() <= 0 )
            continue;

        // find all lines that are perpendicular
        for( size_t j = 0; j < horiz.size(); j++ ) {
            LineSegmentF &v2 = horiz.at(j);

//            printf("perp acute angle %d -> %d = %f\n",(int)i,(int)j,acuteAngle(v,v2));
            if( acuteAngle(v,v2) > (PI/2.0-angleTol) ) {
                perpendicular.push_back(v2);
            }
        }

//        printf(" %d perpendicular = %d\n",(int)i,(int)perpendicular.size());

        for( size_t j = 0; j < parallel.size(); j++ ) {
            LineSegmentF &p = parallel.at(j);

            checkSaveRect(v,p,perpendicular);
        }
    }
}

const std::vector<LineSegRect>& RectangleFinder::getRectangles() {
    return foundRect;
}

void RectangleFinder::segmentByAxis( const std::vector<LineSegmentF>& lines )
{
    vert.clear();
    horiz.clear();

    for( size_t i = 0; i < lines.size(); ++i ) {
        const LineSegmentF &l = lines.at(i);

        double dy = abs(l.y1-l.y0);
        double dx = abs(l.x1-l.x0);

        double angle = atan2(dy,dx);

        //printf("  angle = %f dx = %f dy = %f\n",angle,dx,dy);

        // see if its ambiguous if its horizontal or verticle
        if( abs(angle - PI/4.0) < 0.1 ) {
            continue;
        }

        if( dy > dx ) {
            // have the point closest y = 0 is first
            if( l.y1 < l.y0 )
                vert.push_back(swapEndPoints(l));
            else
                vert.push_back(l);
        } else {
            // have the point closest x = 0 is first
            if( l.x1 < l.x0 )
                horiz.push_back(swapEndPoints(l));
            else
                horiz.push_back(l);
        }
    }
}

void RectangleFinder::checkSaveRect( const LineSegmentF& side0 ,
                                     const LineSegmentF& side1 ,
                                     const std::vector<LineSegmentF>& perpendicular )
{
    // figure out which line is on the left/right
    LineSegmentF left;
    LineSegmentF right;

    if( side0.x0 < side1.x0 ) {
        left = side0;
        right = side1;
    } else {
        left = side1;
        right = side0;
    }

//    printf(" left  = %6.2f %6.2f %6.2f %6.2f\n",left.x0,left.y0,left.x1,left.y1);
//    printf(" right = %6.2f %6.2f %6.2f %6.2f\n",right.x0,right.y0,right.x1,right.y1);
//    printf("\n");

    // create a list of lines that are near the top and bottom
    std::vector<LineSegmentF> top;
    std::vector<LineSegmentF> bottom;

    for( size_t i = 0; i < perpendicular.size(); ++i ) {
        const LineSegmentF& l = perpendicular.at(i);

//        printf(" candidate %d = %6.2f %6.2f %6.2f %6.2f\n",(int)i,l.x0,l.y0,l.x1,l.y1);

        // see if it is the top line
        if( abs(l.y0-left.y0) < abs(l.y0-left.y1)) {
            // make sure its close enough to both sides
            if( abs(l.x0-left.x0) < closeTol && abs(l.x1-right.x0) < closeTol &&
                abs(l.y0-left.y0) < closeTol && abs(l.y1-right.y0) < closeTol )
                top.push_back(l);
        } else {
            // make sure its close enough to both sides
            if( abs(l.x0-left.x1) < closeTol && abs(l.x1-right.x1) < closeTol &&
                abs(l.y0-left.y1) < closeTol && abs(l.y1-right.y1) < closeTol )
                bottom.push_back(l);
        }
    }

//    printf("TOTAL VALID top = %d bottom = %d\n",(int)top.size(),(int)bottom.size());

    // create targets from all combinations of valid tops and bottoms
    for( size_t i = 0; i < top.size(); ++i ) {
        LineSegmentF &t = top.at(i);

        for( size_t j = 0; j < bottom.size(); ++j ) {
            LineSegmentF &b = bottom.at(j);

            addRectangle(left,right,t,b);
        }
    }
}

void RectangleFinder::addRectangle( LineSegmentF &l , LineSegmentF &r, LineSegmentF &t, LineSegmentF &b )
{
    LineSegRect rect;
    rect.l = l;
    rect.r = r;
    rect.t = t;
    rect.b = b;

    foundRect.push_back(rect);
}

